/*
	tello石景山飞行试验速度控制算法
	算法采用P控制进行定点跟踪，采用人工势场法进行实时避障与机间避碰
	算法改进为用类的形式进行实例储存
*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Empty.h"
#include "tf/transform_datatypes.h"
#include "geometry_msgs/Twist.h"
#include "geometry_msgs/PoseStamped.h"
#include "geometry_msgs/PoseArray.h"
#include <math.h>
#include <iostream>
#include <vector>
#define PI 3.1415926
using namespace std;

double t = 0, tstart = 0;
int flag = 0;
double K = -0.6;
double p[3];// 位置跟踪项,位置避障偏移量
double obstacle[50][3] = {0};//用于储存障碍物位置的坐标,半径

int n1 = 4, n2 = 0;//n1代表编队飞机的个数，n2代表障碍物的个数

double carpos[50][2] = {0};//用于储存小车的位置

double K_rep=1.0;//斥力场系数
double ro1 = 0.4, ro2 = 0.3;//机间避障与障碍物避障的作用范围

double r1 = 1.0, r2 = 1.5, r3 = 0.6, w = 0.1745;//定义偏移半径与角速度
double vmax = 0.5;//最大速度
double t1 = 109, t2 = 181, t3 = 80;//t1,t2代表旋转时间的两个时间点，t3代表节点丢失的时间点
int command = 0;
class Tello
{
    string ID; //tello的ID编号
	
    public:
	double pos[6] = {0}; //tello的位姿
    double h[3] = {0}, v[3] = {0}; //编队的偏移量与速度补偿项
    double F[3] = {0}, Fp[3] = {0};//避碰补偿项
    double D[50], A[50], Fx[50], Fy[50], Fpx[50], Fpy[50];//机间距离，斥力函数等
    double dp_ob[50], ap_ob[50];//用于储存飞行中心与障碍物之间的距离，以及二者之间的夹角
	geometry_msgs::Twist cmd_vel;//速度控制量
	ros::Subscriber pub_sub;
	ros::Publisher takeoff_pub,vel_pub,land_pub;

	//输入无人机ID
	void setID(int i)
	{
		ID = to_string(i);
	}
	//获取无人机ID
	string getID()
	{
		return ID;
	}
	//设计编队队形
    void setform(double x, double y, double vx, double vy)
    {
        h[0] = x, h[1] = y;
        v[0] = vx, v[1] = vy;
    }
	//订阅无人机位置信息
	void cb_readpos(const geometry_msgs::PoseStamped::ConstPtr &msg)
	{
		geometry_msgs::PoseStamped Pos;
		double Roll, Pitch, Yaw;
		Pos = *msg;
		pos[0] = Pos.pose.position.x;
		pos[1] = Pos.pose.position.y;
		pos[2] = Pos.pose.position.z;

		tf::Quaternion quat; // 将四元数转换为偏航角等信息
		tf::quaternionMsgToTF(Pos.pose.orientation, quat);
		tf::Matrix3x3(quat).getRPY(Roll, Pitch, Yaw);
		Yaw = Yaw * 180 / 3.14159;
		Yaw = atan2(sin(Yaw), cos(Yaw));
		pos[3] = Yaw;
	}
	//实例化订阅者对象
	void initsubscriber(ros::NodeHandle nh)
	{
		string topic = "/tello" + ID;
		pub_sub = nh.subscribe(topic + "/pose",10,&Tello::cb_readpos,this);//这里必须加一个this来将对象成员绑定到当前实例
	}
	void initpublisher(ros::NodeHandle nh)
	{
		string topic = "/tello" + ID;
		// takeoff_pub = nh.advertise<std_msgs::Empty>(topic + "/takeoff",10);
		// land_pub = nh.advertise<std_msgs::Empty>(topic + "/land",10);
		vel_pub = nh.advertise<geometry_msgs::Twist>(topic + "/cmd",10);
	}
	//计算控制输入,并限制速度
	void vel_control(double n)
	{
		// cmd_vel.linear.x = 1, cmd_vel.linear.y = 2;
		cmd_vel.linear.x = K * (pos[0] - p[0] - h[0]) + v[0] + F[0] + Fp[0];
		cmd_vel.linear.y = K * (pos[1] - p[1] - h[1]) + v[1] + F[1] + Fp[1];
		if(abs(cmd_vel.linear.x) > n)
			cmd_vel.linear.x = (cmd_vel.linear.x > 0) ? n : -n;
		if(abs(cmd_vel.linear.y) > n)
			cmd_vel.linear.y = (cmd_vel.linear.y > 0) ? n : -n;
		// cout<<cmd_vel.linear.x<<" "<<cmd_vel.linear.y<<endl;
	}
};
std::vector<Tello*> uavs;

void poscmd(const geometry_msgs::PoseStamped::ConstPtr & msg)//接收中心位置点的位置坐标
{
    geometry_msgs::PoseStamped p_cmd;
	p_cmd = *msg;//msg是数据整体，并不是一个指针
	p[0] = p_cmd.pose.position.x;
	p[1] = p_cmd.pose.position.y;
}

void cb_command(const std_msgs::Int32::ConstPtr &msg)//接收指令信息
{
	std_msgs::Int32 cmd_data;
	cmd_data = *msg;
	command = cmd_data.data;
	cout << "receive command: " << command << endl;
	if (command == 2 && flag == 0)
	{
		tstart = ros::Time::now().toSec();
		flag = 1;
	}
}

void cb_obstaclepos(const geometry_msgs::PoseArray::ConstPtr &msg) // 接收所有障碍物位置的回调函数
{
	geometry_msgs::PoseArray pos;
	pos = *msg;
	n2 = pos.poses.size();
	for (int i = 0; i < n2; i++)
	{
		obstacle[i][0] = pos.poses[i].position.x;
		obstacle[i][1] = pos.poses[i].position.y;
		obstacle[i][2] = pos.poses[i].position.z;//代表障碍物半径
	}
	// cout<<"收到障碍物位置信息,共有"<<n2<<"个障碍物"<<endl;
}

void cb_carpos0(const geometry_msgs::PoseStamped::ConstPtr &msg)//读取小车0位置的回调函数
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	carpos[0][0] = pos.pose.position.x;
	carpos[0][1] = pos.pose.position.y;
}
void cb_carpos1(const geometry_msgs::PoseStamped::ConstPtr &msg)//读取小车1位置的回调函数
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	carpos[1][0] = pos.pose.position.x;
	carpos[1][1] = pos.pose.position.y;
}
void cb_carpos2(const geometry_msgs::PoseStamped::ConstPtr &msg)//读取小车2位置的回调函数
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	carpos[2][0] = pos.pose.position.x;
	carpos[2][1] = pos.pose.position.y;
}
void cb_carpos3(const geometry_msgs::PoseStamped::ConstPtr &msg)//读取小车3位置的回调函数
{
	geometry_msgs::PoseStamped pos;
	pos = *msg;
	carpos[3][0] = pos.pose.position.x;
	carpos[3][1] = pos.pose.position.y;
}

// 计算避障
void vepul()
{
	int i = 0, j = 0;//不要把这个变成全局变量
	for(i = 0;i < n1;i ++)//记得每次计算将变量初始化
	{
		uavs[i]->F[0] = 0, uavs[i]->F[1] = 0;
		uavs[i]->Fp[0] = 0, uavs[i]->Fp[1] = 0;
	}
	// 计算无人机机间的斥力
	for (i = 0; i < n1; i++)
	{
		for(j = 0;j < n1 && j != i;j ++)
		{
			uavs[i]->D[j] = sqrt(pow(uavs[i]->pos[0] - uavs[j]->pos[0], 2) + pow(uavs[i]->pos[1] - uavs[j]->pos[1], 2));
			uavs[i]->A[j] = atan2(uavs[j]->pos[1] - uavs[i]->pos[1],uavs[j]->pos[0] - uavs[i]->pos[0]);
		}
	}
	for (i = 0; i < n1; i++)
	{
		for (j = 0; j < n1 && j != i; j++)
		{
			if (uavs[i]->D[j] < ro1)
			{
				uavs[i]->Fx[j] = -K_rep * (1 / uavs[i]->D[j] - 1 / ro1) * (1 / uavs[i]->D[j]) * cos(uavs[i]->A[j]);
				uavs[i]->Fy[j] = -K_rep * (1 / uavs[i]->D[j] - 1 / ro1) * (1 / uavs[i]->D[j]) * sin(uavs[i]->A[j]);
				cout << uavs[i]->getID() << "与" << uavs[j]->getID() << "即将相撞" << endl;
				if (uavs[i]->pos[0] >= uavs[j]->pos[0])
					uavs[i]->Fx[j] += 0.2; // 以0.2m/s的速度侧移离开障碍物范围
				else
					uavs[i]->Fx[j] -= 0.2;
			}
			else
			{
				uavs[i]->Fx[j] = 0;
				uavs[i]->Fy[j] = 0;
			}
			uavs[i]->F[0] += uavs[i]->Fx[j], uavs[i]->F[1] += uavs[i]->Fy[j];
		}
		// cout<<"tello"<<i<<":"<<uavs[i]->F[0]<<" "<<uavs[i]->F[0];
	}

	// 计算无人机与障碍物之间的斥力
	for (i = 0; i < n1; i++)
	{
		for (j = 0; j < n2; j++)
		{
			uavs[i]->dp_ob[j] = sqrt(pow(uavs[i]->pos[0] - obstacle[j][0], 2) + pow(uavs[i]->pos[1] - obstacle[j][1], 2));
			uavs[i]->ap_ob[j] = atan2(obstacle[j][1] - uavs[i]->pos[1], obstacle[j][0] - uavs[i]->pos[0]);
			// cout<<"dp_ob:"<<uavs[i]->dp_ob[j]<<"碰撞阈值:"<<ro2 + obstacle[i][2]<<endl;
			if (uavs[i]->dp_ob[j] <= ro2 + obstacle[j][2]) // 如果进入到可能与障碍物碰撞的范围
			{
				uavs[i]->Fpx[j] = -K_rep * (1 / uavs[i]->dp_ob[j] - 1 / (ro2 + obstacle[j][2])) * (1 / (uavs[i]->dp_ob[j] - obstacle[j][2])) * cos(uavs[i]->ap_ob[j]);
				uavs[i]->Fpy[j] = -K_rep * (1 / uavs[i]->dp_ob[j] - 1 / (ro2 + obstacle[j][2])) * (1 / (uavs[i]->dp_ob[j] - obstacle[j][2])) * sin(uavs[i]->ap_ob[j]);
				// if (uavs[i]->pos[0] >= obstacle[i][0])
				// 	uavs[i]->Fpx[j] += 0.2; // 以0.2m/s的速度侧移离开障碍物范围
				// else
				// 	uavs[i]->Fpx[j] -= 0.2;
				// if (uavs[i]->pos[1] >= obstacle[i][1])
				// 	uavs[i]->Fpy[j] += 0.1; // 以0.1m/s的速度纵向离开障碍物范围
				// else
				// 	uavs[i]->Fpy[j] -= 0.1;
			}
			else
			{
				uavs[i]->Fpx[j] = 0;
				uavs[i]->Fpy[j] = 0;
			}
			uavs[i]->Fp[0] += uavs[i]->Fpx[j], uavs[i]->Fp[1] += uavs[i]->Fpy[j];
		}
		// cout<<uavs[i]->Fp[0]<<" "<<uavs[i]->Fp[1]<<endl;
	}
}

int main(int argc, char **argv)
{
	ros::init(argc, argv, "tello_vel_v2");
	ros::NodeHandle nh("");
	int i, j;
	geometry_msgs::PoseStamped uav_center;//编队中心从外部算法接收
	//用堆的方式存储Tello类，这样内存会更加安全
	// 在每次循环中，通过 new Tello() 动态分配一个 Tello 类型的对象，并将其指针添加到 uavs中。
	// uavs 中存储了多个 Tello 对象的指针，这样可以方便地管理和访问多个对象。
	for(i = 0;i < n1;i ++)
		uavs.push_back(new Tello());
	//从地面站订阅tello的当前位置（mocap - xgc - tello）
	for(i = 0;i < n1;i ++)//定义uavs_ros通信相关的变量
	{
		uavs[i]->setID(i + 1);//先置入ID信息,也可以单独置入
		uavs[i]->initsubscriber(nh);
		uavs[i]->initpublisher(nh);
	}
	//发布当前的中心点位置信息(指令中心位置)
	ros::Publisher center_pub = nh.advertise<geometry_msgs::PoseStamped>("/tello_center", 10);

	//订阅障碍物位置
	ros::Subscriber obstacle_sub = nh.subscribe("/obstacles",10,cb_obstaclepos);

	//订阅小车位置
	ros::Subscriber car_sub0 = nh.subscribe("/car1",10,cb_carpos0);
	ros::Subscriber car_sub1 = nh.subscribe("/car2",10,cb_carpos1);
	ros::Subscriber car_sub2 = nh.subscribe("/car3",10,cb_carpos2);
	ros::Subscriber car_sub3 = nh.subscribe("/car4",10,cb_carpos3);

	ros::Subscriber pos_cmd = nh.subscribe("pos_cmd", 10, poscmd);
	ros::Subscriber cmd_sub = nh.subscribe("/command", 10, cb_command);//订阅控制命令信息

	ros::Rate loop_rate(50);

	while (ros::ok())
	{
		double t = ros::Time::now().toSec() - tstart; 
		uav_center.pose.position.x = 0, uav_center.pose.position.y = 0;
		for(i = 0;i < n1;i ++)
		{
			uav_center.pose.position.x += uavs[i]->pos[0]/n1;//中心点计算代码
			uav_center.pose.position.y += uavs[i]->pos[1]/n1; 
		}
		center_pub.publish(uav_center);//计算四架飞机的中心并发布，检测飞行是否稳定

		// if (command == 1) // 起飞
		// {
		// 	// cout<<"起飞"<<endl;
		// 	std_msgs::Empty takeoff_cmd;
		// 	for(i = 0;i < n1;i ++)
		// 	{
		// 		uavs[i]->takeoff_pub.publish(takeoff_cmd);
		// 	}
		// }
		if (command == 2)
		{
			double hx,hy,vx,vy;
			//外部的位置指令开始移动，四机跟随中心点开始运动
			if (t <= t1)
			{
				for(i = 0;i < n1;i ++)
				{
					hx = r1*cos(2*i*PI/n1), hy = r1*sin(2*i*PI/n1);
					vx = 0, vy = 0;
					uavs[i]->setform(hx,hy,vx,vy);
					//判断是否需要避障
					vepul();
					//计算输入
					uavs[i]->vel_control(vmax);
				}
			}
			//四机到达指定位置，开始进行盘旋检测(t1,t2根据外部轨迹信息进行确定)
			else if (t > t1 && t <= t2)
			{
				r3 = 1.0;
				for(i = 0;i < n1;i ++)
				{
					hx = r2 * cos(w * (t - t1) + 2*i * PI / n1), hy = r2 * sin(w * (t - t1) + 2*i * PI / n1);
					vx = -r2 * w * sin(w * (t - t1) + 2*i * PI / n1), vy = r2 * w * cos(w * (t - t1) + 2*i * PI / n1);
					uavs[i]->setform(hx,hy,vx,vy);
					//判断是否需要避障
					vepul();
					//计算输入
					uavs[i]->vel_control(vmax);
				}
			}
			//四机完成检测任务，跟车离开
			else
			{
				for(i = 0;i < n1;i ++)
				{
					hx = r1*cos(2*i*PI/n1), hy = r1*sin(2*i*PI/n1);
					vx = 0, vy = 0;
					uavs[i]->setform(hx,hy,vx,vy);
					//判断是否需要避障
					vepul();
					//计算输入
					uavs[i]->vel_control(vmax);
				}
			}
			for(i = 0;i<n1;i++)
				uavs[i]->vel_pub.publish(uavs[i]->cmd_vel); 
		}
		if (command == 3) // 指令速度给0
		{
			for(i=0;i<n1;i++)
			{
				uavs[i]->cmd_vel.linear.x = 0;
				uavs[i]->cmd_vel.linear.y = 0;
				uavs[i]->vel_pub.publish(uavs[i]->cmd_vel);
			}
		}
		// if (command == 9) // 急停降落
		// 	{
		// 		std_msgs::Empty land_cmd;
		// 		for (i = 0; i < n1; i++)
		// 			uavs[i]->land_pub.publish(land_cmd);
		// 	}
		ros::spinOnce();
		loop_rate.sleep();
	}
	// 使用 uavs 中的对象，然后在不再需要时释放内存
	for (i = 0; i < n1; i++)
		delete uavs[i];
	// 清空向量
	uavs.clear();
	return 0;
}